#ifndef FOLLOWER_HPP
#define FOLLOWER_HPP

#include <cmath>

#include "detectserver.hpp"
#include "tost.hpp"

class Follower : public DetectServer
{
public:
    static double interp2(double x, double xmin, double xmax, double ymin, double ymax)
    {
        if(x<=xmin)
            return ymin;
        if(x>=xmax)
            return ymax;
        return ymin+(ymax-ymin)*(std::pow((x-xmin),2))/(std::pow((xmax-xmin),2));
    }


    static double interp(double x, double xmin, double xmax, double ymin, double ymax)
    {
        if(x<=xmin)
            return ymin;
        if(x>=xmax)
            return ymax;
        return ymin+(ymax-ymin)*(x-xmin)/(xmax-xmin);
    }

    static double LowPass(double oldValue, double inputValue, double factor=0.2)
    {
        return oldValue*(1-factor)+inputValue*factor;
    }

    static cv::Vec3d ToSpeed(double xm, double zm)
    {
//        double ymmps=interp2(zm, 0.5, 2, 50, 150);//450

        double ymmps=interp(zm, 0.5, 2, 0, 450);//450
        double radps=interp(atan(xm/zm), -0.4, 0.4, 0.3, -0.3);

        return cv::Vec3d(0, ymmps, radps);
    }

    static cv::Vec3d ToTarget(double xm, double zm)
    {
        double ymmps=interp2(zm/1000+0.3, 0.1, 2, 0, 350);//450
        double radps=interp(atan(xm/zm), -0.8, 0.8, 0.4, -0.4);

        return cv::Vec3d(0, ymmps, radps);
    }

    static void LowPass(cv::Vec3d &inOutValue, cv::Vec3d newValue)
    {
        inOutValue[0]=LowPass(inOutValue[0], newValue[0]);
        inOutValue[1]=LowPass(inOutValue[1], newValue[1]);
        inOutValue[2]=LowPass(inOutValue[2], newValue[2]);
    }





public:
    Follower()
        : DetectServer()
    {

    }

    void Follow()
    {
        Start(4567);

        ToST tst;
        tst.Initial();

        cv::Vec3d speedOut(0,0,0);

        for(size_t i=0;i<10000;i++)
        {
            std::cout<<i<<"\t";

            DetectResultS *pdr=NULL;
            humanBuffer.Read(pdr);
            if(pdr!=NULL)
            {
                cv::Vec3d speed(0,0,0);
                if(!pdr->human.empty())
                 {
                    std::cout<<"human at "<<pdr->human[0].XY()<<"\n"<<std::flush;
                    speed=ToSpeed(pdr->human[0].XY()[0], pdr->human[0].XY()[1]);
                }
                else
                {
                    std::cout<<"no human\n"<<std::flush;
                }

                LowPass(speedOut, speed);
                std::cout<<"speedout="<<speedOut<<"\n"<<std::flush;

                tst.Move(speedOut[0], speedOut[1], speedOut[2]);

                delete pdr;

            }

        }

        tst.Move();
        tst.End();

        End();



    }
};

#endif // FOLLOWER_HPP

